int yuv_TestAdd(void)
{
    AmbaPrint("Adding yuv_TestAdd");

    AppLibVideoAnal_FrmHdlr_Init();
    AmbaTest_RegisterCommand("yuv", yuv_test);

    return 0;
}

int yuv_test(struct _AMBA_SHELL_ENV_s_ *env, int argc, char **argv)
{

    if (strcmp(argv[1],"start") == 0) 
    {
        if (fp != NULL)
        {
            AmbaPrint("yuv test already running !");
            return 0;
        }
        else
        {
            if (argv[2] == NULL || argv[2][0] == 0)
            {
                strncpy(yuv_file_name, YUV_DEFAULT_FILE_NAME, sizeof(yuv_file_name));
            }
            else
            {
                strncpy(yuv_file_name, "c:\\", sizeof(yuv_file_name));
                strncat(yuv_file_name, argv[2], sizeof(yuv_file_name));
            }
            AmbaPrint("opening yuv file<%s> ...", yuv_file_name);
            fp = AmbaFS_fopen(yuv_file_name, "w");
            if (fp == NULL)
            {
                int err = 0;
                AmbaFS_GetError(&err);
                AmbaPrint("open file error: %d", err);
                return -1;
            }

            AppLibVideoAnal_FrmHdlr_Register(APPLIB_FRM_HDLR_2ND_YUV, cb_save_yuv);
            AmbaPrint("start yuv test successfully!");
            return 0;
        }
    }
    else if (strcmp(argv[1],"stop") == 0)
    {
        if (fp == NULL)
        {
            AmbaPrint("yuv test is not running now !");
            return 0;
        }
        else
        {
            AppLibVideoAnal_FrmHdlr_UnRegister(APPLIB_FRM_HDLR_2ND_YUV, cb_save_yuv);
            AmbaFS_fclose(fp);
            fp = NULL;
            AmbaPrint("stop yuv test.");
            return 0;
        }
    }
    else
    {
        AmbaPrint("%s<%d>: no such cmd: %s", __FUNCTION__, __LINE__, argv[1]);
        return -1; 
    }

}


#define YUV_DEFAULT_FILE_NAME "c:\\yuv_000.yuv"
static AMBA_FS_FILE *fp = NULL;
static char yuv_file_name[25];
static char yuv_data[1280*720*2];
static char y_buf[1280*720];
static char uv_buf[1280*720];

int cb_save_yuv(UINT32 event, AMP_ENC_YUV_INFO_s* img)
{
    UINT16 col, row;
    char *y_src, *uv_src, *y_dst, *uv_dst;

    //AmbaPrint("hello kitty: %s():%d, event: %x", __FUNCTION__, __LINE__, event);

    //if (event == AMP_ENC_EVENT_LIVEVIEW_DCHAN_YUV_READY)
    if (event == AMP_ENC_EVENT_VCAP_2ND_YUV_READY)
    {
        if (img->colorFmt == AMP_YUV_420)
        {
            #if 0
            AmbaPrint("img->ChannelID = %d, img->colorFmt = %d, img->ySize = %d", 
                        img->ChannelID, img->colorFmt, img->ySize);

            AmbaPrint("img->pitch = %d, img->width = %d, img->height = %d", 
                        img->pitch, img->width, img->height);
            //AmbaPrint("cb_save_yuv: ++++ AMP_YUV_420");
            return 0;
            #endif

#if 1 // YUV420SP
            memcpy(y_buf, img->yAddr, img->ySize);
            memcpy(uv_buf, img->uvAddr, img->ySize);
            y_src = y_buf; // img->yAddr;
            uv_src = uv_buf; //img->uvAddr;
            y_dst = yuv_data;
            uv_dst = y_dst + img->width * img->height;

            for (row = 0; row < img->height; row++)
            {
                memcpy(y_dst, y_src, img->width);
                y_src += img->pitch;
                y_dst += img->width;
            }

            col = img->height >> 1;
            for (row = 0; row < col; row++)
            {
                memcpy(uv_dst, uv_src, img->width);
                uv_src += img->pitch;
                uv_dst += img->width;
            }

            AmbaFS_fwrite(yuv_data, img->width * img->height * 3 / 2, 1, fp);
#endif
        }
        else // AMP_YUV_422
        {
            //AmbaPrint("cb_save_yuv: ++++ AMP_YUV_422");

            #if 0
            // U0 Y0 V0 Y1 U2 Y2 V2 Y3 U4 Y4 V4 …    
            for (row = 0; row < img->height; row++)
            {
                y = img->yAddr + img->pitch * row;
                uv = img->uvAddr + img->pitch * row; //4 fix me
                for (col = 0; col < img->width; col++)
                {
                    if (col & 0x01 == 0)
                    {
                        AmbaFS_fwrite(uv, 1, 1, fp);
                        uv++;
                    }

                    AmbaFS_fwrite(y, 1, 1, fp);
                    y++;
                    #if 1
                    if (col & 0x01 == 0)
                    {
                        AmbaFS_fwrite(uv, 1, 1, fp);
                        uv++;
                    }
                    #endif
                } 
            }
            #else

            /*
            AmbaPrint("img->ChannelID = %d, img->colorFmt = %d, img->ySize = %d", 
                        img->ChannelID, img->colorFmt, img->ySize);

            AmbaPrint("img->pitch = %d, img->width = %d, img->height = %d", 
                        img->pitch, img->width, img->height);
            */

            memcpy(y_buf, img->yAddr, img->ySize);
            memcpy(uv_buf, img->uvAddr, img->ySize);
            y_src = y_buf; // img->yAddr;
            uv_src = uv_buf; //img->uvAddr;
            y_dst = yuv_data;
            uv_dst = y_dst + img->width * img->height;

#if 1 // YUV422SP
            for (row = 0; row < img->height; row++)
            {
                memcpy(y_dst, y_src, img->width);
                memcpy(uv_dst, uv_src, img->width);

                y_src += img->pitch;
                uv_src += img->pitch;
                y_dst += img->width;
                uv_dst += img->width;
            }
#if 0
            // change u and v
            char tmp;
            uv_dst = yuv_data + img->width * img->height;
            int count = img->width * img->height;
            while (count)
            {
                tmp = uv_dst[0];
                uv_dst[0] = uv_dst[1];
                uv_dst[1] = tmp;
                uv_dst +=2;
                count -= 2;
            }

#endif
            AmbaFS_fwrite(yuv_data, img->width * img->height * 2, 1, fp);

#endif

            #endif
        }
    } /* if (event == AMP_ENC_EVENT_LIVEVIEW_DCHAN_YUV_READY) */
    return 0;
}
